//
// Created by champrin on 2022/6/27.
//
#include "Armor.h"
#include "Config.h"

cv::Point2f in = cv::Point2f(640,384);
cv::Point2f out = cv::Point2f(1920,1080);

cv::Point2f transform(cv::Point2f p, cv::Point2f in, cv::Point2f out) {
	return cv::Point2f(p.x * out.x / in.x, p.y * out.y / in.y);
}

void Armor::Transform2P(cv::RotatedRect& r1, cv::RotatedRect& r2) {
	cv::Point2f ps1[4], ps2[4];
	r1.points(ps1);
	r2.points(ps2);
	float xtmp1 = (std::min({ ps1[0].x, ps1[1].x, ps1[2].x, ps1[3].x }) + std::max({ ps1[0].x, ps1[1].x, ps1[2].x, ps1[3].x })) / 2;
	float xtmp2 = (std::min({ ps2[0].x, ps2[1].x, ps2[2].x, ps2[3].x }) + std::max({ ps2[0].x, ps2[1].x, ps2[2].x, ps2[3].x })) / 2;
	float y1 = std::min({ ps1[0].y, ps1[1].y, ps1[2].y, ps1[3].y });
	float y2 = std::max({ ps1[0].y, ps1[1].y, ps1[2].y, ps1[3].y });
	float x1, x2;
	if (xtmp1 > xtmp2) {
		x1 = xtmp2;
		x2 = xtmp1;
	}
	else {
		x1 = xtmp1;
		x2 = xtmp2;
	}
	p1 = cv::Point2f(x1, y1);
	p2 = cv::Point2f(x2, y2);
	pnt = std::vector<cv::Point2f>{
			transform(p1,in,out),
			transform(cv::Point2f(p2.x,p1.y),in,out),
			transform(p2,in,out),
			transform(cv::Point2f(p1.x,p2.y),in,out)
	};
}

void Armor::DrawRectangle(cv::Mat& img, const cv::Scalar& recColor) {
	rectangle(img, p1, p2, recColor);
}

void Armor::SetEnemyColor(EnemyColor enemyColor) {
	this->color = enemyColor;
}

void Armor::SolvePNP() {


	rVec = cv::Mat::zeros(3, 1, CV_64FC1);
	tVec = cv::Mat::zeros(3, 1, CV_64FC1);
	cv::solvePnP(wPnt, pnt, cameraMatrix, distortionCoefficients, rVec, tVec, false,
		cv::SOLVEPNP_UPNP/*P3P,IPNP,UPNP*/);
}

float Armor::GetDistance() {
	/*
	std::cout << wPnt << std::endl;
	std::cout << pnt << std::endl;
	std::cout << cameraMatrix << std::endl;
	std::cout << distortionCoefficients << std::endl;
	std::cout << tVec << std::endl;*/

	double tx = tVec.at<double>(0, 0);
	double ty = tVec.at<double>(1, 0);
	double tz = tVec.at<double>(2, 0);
	return sqrt(tx * tx + ty * ty + tz * tz);
}
